home *** CD-ROM | disk | FTP | other *** search
/ Graphics Plus / Graphics Plus.iso / libs / vogle / prev.lha / matrix.c < prev    next >
Encoding:
C/C++ Source or Header  |  1991-01-29  |  4.6 KB  |  214 lines
  1. #include <stdio.h>
  2. #include <math.h>
  3. #include "art.h"
  4.  
  5. /*
  6.  * mcpy4
  7.  *
  8.  *    copies 4 by 4 matrix m2 into m1
  9.  */
  10. void
  11. mcpy4(m1, m2)
  12.     register float    *m1, *m2;
  13. {
  14.     register float    *fin;
  15.  
  16.     fin = m1 + 16;
  17.     while (m1 != fin)
  18.         *m1++ = *m2++;
  19. }
  20.  
  21. /*
  22.  * mident4
  23.  *
  24.  *    set m to a 4 by 4 identity matrix
  25.  */
  26. void
  27. mident4(m)
  28.     register float    *m;
  29. {
  30.     register float    *fin, *m1;
  31.  
  32.     m1 = m;
  33.     fin = m + 16;
  34.     while (m != fin)
  35.         *m++ = 0;
  36.  
  37.     m1[0] = 1.0;
  38.     m1[5] = 1.0;
  39.     m1[10] = 1.0;
  40.     m1[15] = 1.0;
  41. }
  42.  
  43. /*
  44.  * mmult4
  45.  *
  46.  *    mulitiplies four by four matrix m3 by m2, leaving result in
  47.  * m1.
  48.  *
  49.  */
  50. void
  51. mmult4(m1, m2, m3)
  52.     register float    m1[4][4], m2[4][4], m3[4][4];
  53. {
  54.     m1[0][0] = m2[0][0] * m3[0][0] + m2[0][1] * m3[1][0] + m2[0][2] * m3[2][0] + m2[0][3] * m3[3][0];
  55.     m1[0][1] = m2[0][0] * m3[0][1] + m2[0][1] * m3[1][1] + m2[0][2] * m3[2][1] + m2[0][3] * m3[3][1];
  56.     m1[0][2] = m2[0][0] * m3[0][2] + m2[0][1] * m3[1][2] + m2[0][2] * m3[2][2] + m2[0][3] * m3[3][2];
  57.     m1[0][3] = m2[0][0] * m3[0][3] + m2[0][1] * m3[1][3] + m2[0][2] * m3[2][3] + m2[0][3] * m3[3][3];
  58.     m1[1][0] = m2[1][0] * m3[0][0] + m2[1][1] * m3[1][0] + m2[1][2] * m3[2][0] + m2[1][3] * m3[3][0];
  59.     m1[1][1] = m2[1][0] * m3[0][1] + m2[1][1] * m3[1][1] + m2[1][2] * m3[2][1] + m2[1][3] * m3[3][1];
  60.     m1[1][2] = m2[1][0] * m3[0][2] + m2[1][1] * m3[1][2] + m2[1][2] * m3[2][2] + m2[1][3] * m3[3][2];
  61.     m1[1][3] = m2[1][0] * m3[0][3] + m2[1][1] * m3[1][3] + m2[1][2] * m3[2][3] + m2[1][3] * m3[3][3];
  62.     m1[2][0] = m2[2][0] * m3[0][0] + m2[2][1] * m3[1][0] + m2[2][2] * m3[2][0] + m2[2][3] * m3[3][0];
  63.     m1[2][1] = m2[2][0] * m3[0][1] + m2[2][1] * m3[1][1] + m2[2][2] * m3[2][1] + m2[2][3] * m3[3][1];
  64.     m1[2][2] = m2[2][0] * m3[0][2] + m2[2][1] * m3[1][2] + m2[2][2] * m3[2][2] + m2[2][3] * m3[3][2];
  65.     m1[2][3] = m2[2][0] * m3[0][3] + m2[2][1] * m3[1][3] + m2[2][2] * m3[2][3] + m2[2][3] * m3[3][3];
  66.     m1[3][0] = m2[3][0] * m3[0][0] + m2[3][1] * m3[1][0] + m2[3][2] * m3[2][0] + m2[3][3] * m3[3][0];
  67.     m1[3][1] = m2[3][0] * m3[0][1] + m2[3][1] * m3[1][1] + m2[3][2] * m3[2][1] + m2[3][3] * m3[3][1];
  68.     m1[3][2] = m2[3][0] * m3[0][2] + m2[3][1] * m3[1][2] + m2[3][2] * m3[2][2] + m2[3][3] * m3[3][2];
  69.     m1[3][3] = m2[3][0] * m3[0][3] + m2[3][1] * m3[1][3] + m2[3][2] * m3[2][3] + m2[3][3] * m3[3][3];
  70. }
  71.  
  72. /*
  73.  * minv4
  74.  *
  75.  *    find the inverse of the 4 by 4 matrix b using gausian elimination
  76.  * and return it in a.
  77.  */
  78. minv4(a, b)
  79.     matrix    a, b;
  80. {
  81.     float    val, val2;
  82.     int    i, j, k, ind;
  83.     matrix    tmp;
  84.  
  85.     mident4(a);
  86.  
  87.     mcpy4(tmp, b);
  88.  
  89.     for (i = 0; i != 4; i++) {
  90.  
  91.         val = tmp[i][i];        /* find pivot */
  92.         ind = i;
  93.         for (j = i + 1; j != 4; j++) {
  94.             if (fabs(tmp[j][i]) > fabs(val)) {
  95.                 ind = j;
  96.                 val = tmp[j][i];
  97.             }
  98.         }
  99.  
  100.         if (ind != i) {            /* swap columns */
  101.             for (j = 0; j != 4; j++) {
  102.                 val2 = a[i][j];
  103.                 a[i][j] = a[ind][j];
  104.                 a[ind][j] = val2;
  105.                 val2 = tmp[i][j];
  106.                 tmp[i][j] = tmp[ind][j];
  107.                 tmp[ind][j] = val2;
  108.             }
  109.         }
  110.  
  111.         if (val == 0.0)
  112.             fatal("art: singular matrix in minv4.\n");
  113.  
  114.         for (j = 0; j != 4; j++) {
  115.             tmp[i][j] /= val;
  116.             a[i][j] /= val;
  117.         }
  118.  
  119.         for (j = 0; j != 4; j++) {    /* eliminate column */
  120.             if (j == i)
  121.                 continue;
  122.             val = tmp[j][i];
  123.             for (k = 0; k != 4; k++) {
  124.                 tmp[j][k] -= tmp[i][k] * val;
  125.                 a[j][k] -= a[i][k] * val;
  126.             }
  127.         }
  128.     }
  129. }
  130.  
  131. /*
  132.  * minv3x3
  133.  *
  134.  *    calculate the inverse of a 3x3 matrix b and put it in a.
  135.  */
  136. void
  137. minv3x3(a, b)
  138.     mat3x3    a, b;
  139. {
  140.     float    det;
  141.  
  142.     /*
  143.      * compute the adjoint
  144.      */
  145.     a[0][0] = b[1][1] * b[2][2] - b[1][2] * b[2][1];
  146.     a[0][1] = b[0][2] * b[2][1] - b[0][1] * b[2][2];
  147.     a[0][2] = b[0][1] * b[1][2] - b[0][2] * b[1][1];
  148.  
  149.     a[1][0] = b[1][2] * b[2][0] - b[1][0] * b[2][2];
  150.     a[1][1] = b[0][0] * b[2][2] - b[0][2] * b[2][0];
  151.     a[1][2] = b[0][2] * b[1][0] - b[0][0] * b[1][2];
  152.  
  153.     a[2][0] = b[1][0] * b[2][1] - b[1][1] * b[2][0];
  154.     a[2][1] = b[0][1] * b[2][0] - b[0][0] * b[2][1];
  155.     a[2][2] = b[0][0] * b[1][1] - b[0][1] * b[1][0];
  156.  
  157.     /*
  158.      * compute the determinate
  159.      */
  160.     det = a[0][0] * b[0][0] + a[1][0] * b[0][1] + a[2][0] * b[0][2];
  161.  
  162.     if (det == 0.0)
  163.         fatal("art: singular matrix in minv4.\n");
  164.  
  165.     /*
  166.      * divide through by the determinate
  167.      */
  168.     a[0][0] /= det;
  169.     a[0][1] /= det;
  170.     a[0][2] /= det;
  171.  
  172.     a[1][0] /= det;
  173.     a[1][1] /= det;
  174.     a[1][2] /= det;
  175.  
  176.     a[2][0] /= det;
  177.     a[2][1] /= det;
  178.     a[2][2] /= det;
  179. }
  180.  
  181.  
  182. /*
  183.  * smult4
  184.  *
  185.  *    multiply a 4 by 4 matrix by the scalar s
  186.  *
  187.  */
  188. smult4(m, s)
  189.     register float    *m;
  190.     register float    s;
  191. {
  192.     register float    *fin;
  193.  
  194.     fin = m + 16;
  195.     while (m != fin)
  196.         *m++ *= s;
  197. }
  198.  
  199. /*
  200.  * printmatrix
  201.  *
  202.  *    print out a 4 by 4 matrix.
  203.  */
  204. printmatrix(m)
  205.     matrix    m;
  206. {
  207.     printf("matrix %x\n", m);
  208.  
  209.     printf("%e %e %e %e\n", m[0][0], m[0][1], m[0][2], m[0][3]);
  210.     printf("%e %e %e %e\n", m[1][0], m[1][1], m[1][2], m[1][3]);
  211.     printf("%e %e %e %e\n", m[2][0], m[2][1], m[2][2], m[2][3]);
  212.     printf("%e %e %e %e\n", m[3][0], m[3][1], m[3][2], m[3][3]);
  213. }
  214.